//All includes
#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "roboard_drivers/Encoder.h"
#include "roboard_drivers/Motor.h"
#include <unistd.h>
//#include "roboard_drivers/imu.h"
#include <stdio.h>
#include "saint_nr_9/topic1.h"

//All defines
#ifndef TICKS_P_DEGREE
        #define TICKS_P_DEGREE  1.45
#endif

#ifndef METERS_P_TICKS
        #define METERS_P_TICKS  0.00125
#endif

#ifndef SPEED
        #define SPEED   0.4
#endif

using namespace roboard_drivers;

//All forward decalatations
int dist(int a, int b);
void forward(double distance, int sl, int sr);
void backward(double distance, int sl, int sr);
void t_left(double angel, int sl, int sr);
void t_right(double angel, int sl, int sr);

//All global variables, whatever works bro!
ros::Subscriber enc_sub;
ros::Publisher  mot_pub;
ros::Publisher  int_pub;
ros::Subscriber mot_sub;
Motor motor; //Fult

//void recive_command(const std_msgs::Int32::ConstPtr &command){
//	printf("lol");
//
//}

// Called when the message is received.
void sub_callback (const saint_nr_9::topic1 &msg)
{
    // Do something with the message.
    ROS_INFO("I receive: %i\n", msg.lc);
}

int main(int argc, char **argv){
	//ros::Ros.topic.new("/serial/motor_control");        

	ros::init(argc, argv, "topicTest");//Creates a node named "MotorControl"
        ros::NodeHandle n;
        //enc_sub = n.subscribe("/serial/encoder", 1000, recive_encoder);//when "/encoder" topic is revieved call recive_encoder function
        //mot_sub = n.subscribe("/serial/motor_control", 10, recive_command);
        //mot_sub = n.subscribe("/serial/motor_control", 10, recive_command);

	// Define a subscriber object instance.
	ros::Subscriber sub = n.subscribe("/serial/motor_control", 10 , sub_callback);

	ros::Rate loop_rate(100);
	
	ros::spin();

//	while(ros::ok()){
//		ros::spinOnce();
		//printf("test\n");
         //       loop_rate.sleep();
	//}

        return 0;
}

